#include "vofa_frame/vofa_frame.hpp"
#include <fcntl.h>
#include <unistd.h>
#include <netinet/tcp.h>
#include <sys/ioctl.h>
#include <iostream>

VofaFrame::VofaFrame(std::string vofa_ip, int vofa_port)
{
    use_tcp = true;
    last_connect_time_point = std::chrono::steady_clock::now();

    memset(&destAddr, 0, sizeof(destAddr));
    destAddr.sin_family = AF_INET;
    destAddr.sin_port = htons(vofa_port);
    inet_pton(AF_INET, vofa_ip.c_str(), &destAddr.sin_addr);

    buffer_.reserve(1024);
}

VofaFrame::~VofaFrame()
{
    if (udp_sock >= 0)
        close(udp_sock);
    if (tcp_sock >= 0)
        close(tcp_sock);
}

void VofaFrame::push(float value)
{
    uint8_t bytes[4];
    std::memcpy(bytes, &value, 4);
    buffer_.insert(buffer_.end(), bytes, bytes + 4);
}

void VofaFrame::push(float v1, float v2)
{
    push(v1);
    push(v2);
}

void VofaFrame::push(float v1, float v2, float v3)
{
    push(v1);
    push(v2);
    push(v3);
}

void VofaFrame::push(const Eigen::Vector3f& vec)
{
    for (int i = 0; i < 3; ++i)
    {
        push(vec[i]);
    }
}

void VofaFrame::push(const Eigen::Vector3d& vec)
{
    for (int i = 0; i < 3; ++i)
    {
        push(vec[i]);
    }
}

std::vector<uint8_t> VofaFrame::getFrame()
{
    std::vector<uint8_t> frame = buffer_;
    const uint8_t tail[4] = {0x00, 0x00, 0x80, 0x7f};
    frame.insert(frame.end(), tail, tail + 4);
    return frame;
}

void VofaFrame::reset()
{
    buffer_.clear();
}

void VofaFrame::connect_udp()
{
    if (udp_sock >= 0)
        return;

    udp_sock = socket(AF_INET, SOCK_DGRAM, 0);
    if (udp_sock < 0)
    {
        printf("UDP Socket creation failed");
        return;
    }

    int flags = fcntl(udp_sock, F_GETFL, 0);
    fcntl(udp_sock, F_SETFL, flags | O_NONBLOCK);

    flags = fcntl(udp_sock, F_GETFL, 0);
    if (!(flags & O_NONBLOCK))
    {
        printf("⚠️ UDP Socket is blocking");
    }

    printf("UDP Socket created");
}

void VofaFrame::connect_tcp()
{
    if (tcp_sock >= 0)
        return;

    tcp_sock = socket(AF_INET, SOCK_STREAM, 0);
    if (tcp_sock < 0)
    {
        // ERROR("TCP Socket creation failed");
        tcp_connected = false;
        return;
    }

    int flags = fcntl(tcp_sock, F_GETFL, 0);
    fcntl(tcp_sock, F_SETFL, flags | O_NONBLOCK);

    int ret = connect(tcp_sock, (sockaddr*)&destAddr, sizeof(destAddr));
    if (ret == 0)
    {
        // INFO("TCP Connected immediately");
        tcp_connected = true;
        tcp_ready_to_send = false;
        tcp_connected_time = std::chrono::steady_clock::now();
    }
    else if (ret < 0 && errno == EINPROGRESS)
    {
        // INFO("TCP connecting (non-blocking)...");
        tcp_connected = false;
    }
    else
    {
        // ERROR("TCP connect() failed");
        close(tcp_sock);
        tcp_sock = -1;
        tcp_connected = false;
    }
}

void VofaFrame::send()
{
    std::vector<uint8_t> frame_raw = getFrame();

    if (use_tcp)
    {
        auto now = std::chrono::steady_clock::now();
        auto ms_since_last_connect = std::chrono::duration_cast<std::chrono::milliseconds>(now - last_connect_time_point).count();

        if (tcp_sock < 0 || (!tcp_connected && ms_since_last_connect >= reconnect_interval_ms))
        {
            if (tcp_sock >= 0)
            {
                close(tcp_sock);
                tcp_sock = -1;
                tcp_connected = false;
                // WARN("TCP reconnect: closing old socket");
            }

            connect_tcp();
            last_connect_time_point = now;
        }

        if (tcp_sock >= 0 && !tcp_connected)
        {
            int err = 0;
            socklen_t len = sizeof(err);
            getsockopt(tcp_sock, SOL_SOCKET, SO_ERROR, &err, &len);
            if (err == 0)
            {
                tcp_connected = true;
                tcp_ready_to_send = false;
                tcp_connected_time = std::chrono::steady_clock::now();
                // INFO("TCP connected (after EINPROGRESS)");
            }
            else if (err != EINPROGRESS && err != EALREADY)
            {
                close(tcp_sock);
                tcp_sock = -1;
                tcp_connected = false;
                // WARN("TCP connect failed, will retry");
            }
        }

        if (tcp_connected)
        {
            if (std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - tcp_connected_time).count() > tcp_connect_delay_ms) {
                tcp_ready_to_send = true;
            } else {
                tcp_ready_to_send = false;
            }
            if (tcp_ready_to_send) {
                ssize_t sent = ::send(tcp_sock, frame_raw.data(), frame_raw.size(), MSG_DONTWAIT | MSG_NOSIGNAL);
                if (sent < 0)
                {
                    // WARN("TCP send failed, closing socket");
                    close(tcp_sock);
                    tcp_sock = -1;
                    tcp_connected = false;
                }
            }
        }
        else
        {
            // WARN("TCP not connected, skip send");
        }
    }
    else
    {
        if (udp_sock < 0)
            connect_udp();

        ssize_t sent = sendto(udp_sock, frame_raw.data(), frame_raw.size(), 0, (sockaddr*)&destAddr, sizeof(destAddr));
        if (sent < 0)
        {
            printf("UDP sendto failed");
        }
    }

    reset();
}
